//这里创建了两个节点实现，收集力信息又开了一个节点
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/twist_stamped.hpp>  // 添加TwistStamped消息类型
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <cmath>
#include <vector>
#include <memory>  // 添加用于智能指针
#include <atomic>  // 添加 atomic 用于线程安全标志

std::atomic<bool> trajectory_completed(false);  // 添加全局标志，表示轨迹是否完成
bool force_sign = true;

void scale_trajectory_speed(moveit::planning_interface::MoveGroupInterface::Plan &plan, double scale)
{
    int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();

    for (size_t i = 0; i < plan.trajectory_.joint_trajectory.points.size(); i++)
    {
        // 将 Duration 转换为秒（double 类型）进行缩放
        double time_in_seconds = plan.trajectory_.joint_trajectory.points[i].time_from_start.sec +
                                 plan.trajectory_.joint_trajectory.points[i].time_from_start.nanosec / 1e9;
        time_in_seconds /= scale; // 时间缩放（1/scale）

        // 将缩放后的时间转换回 Duration
        plan.trajectory_.joint_trajectory.points[i].time_from_start.sec = static_cast<int32_t>(time_in_seconds);
        plan.trajectory_.joint_trajectory.points[i].time_from_start.nanosec =
            static_cast<uint32_t>((time_in_seconds - plan.trajectory_.joint_trajectory.points[i].time_from_start.sec) * 1e9);

        for (int j = 0; j < n_joints; j++)
        {
            plan.trajectory_.joint_trajectory.points[i].velocities[j] *= scale;
            plan.trajectory_.joint_trajectory.points[i].accelerations[j] *= scale * scale;
        }
    }
}


bool returnToMiddle(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    move_group.setNamedTarget("middle");
    moveit::planning_interface::MoveGroupInterface::Plan home_plan;
    bool success = (move_group.plan(home_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (success) {
        move_group.execute(home_plan);
        RCLCPP_INFO(logger, "回到middle位置.");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to return to middle position.");
        return false;
    }
}

bool continueCartesianPath(moveit::planning_interface::MoveGroupInterface &move_group,const std::vector<geometry_msgs::msg::Pose> &original_waypoints,
    rclcpp::Logger logger,moveit::planning_interface::MoveGroupInterface::Plan &new_cartesian_plan)
{
    geometry_msgs::msg::Pose current_pose = move_group.getCurrentPose().pose;
    std::vector<geometry_msgs::msg::Pose> remaining_waypoints;
    double min_distance = std::numeric_limits<double>::max();
    size_t closest_index = 0;
    
    for (size_t i = 0; i < original_waypoints.size(); ++i) {
        double dx = current_pose.position.x - original_waypoints[i].position.x;
        double dy = current_pose.position.y - original_waypoints[i].position.y;
        double dz = current_pose.position.z - original_waypoints[i].position.z;
        double distance = std::sqrt(dx*dx + dy*dy + dz*dz);
        RCLCPP_INFO(logger, "distance: %f", distance);
        if (distance < min_distance) {
            min_distance = distance;
            closest_index = i;
        }
    }
    RCLCPP_INFO(logger, "最近的路点索引: %zu", closest_index);
    
    for (size_t i = closest_index + 1; i < original_waypoints.size(); ++i) {
        remaining_waypoints.push_back(original_waypoints[i]);
    }
    
    if (remaining_waypoints.empty()) {
        RCLCPP_INFO(logger, "没有剩余的路径点");
        return false;
    }
    move_group.setStartStateToCurrentState();
    double fraction = move_group.computeCartesianPath(remaining_waypoints,0.01,0.0,new_cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "剩余轨迹完成 %.2f%%", fraction * 100.0);
    return fraction >= 0.8;
}

// 定义一个类来处理力数据的订阅
class ForceSubscriber : public rclcpp::Node {
public:
    ForceSubscriber() : Node("force_subscriber") {
        // 创建订阅者，订阅 /force_data 话题
        subscription_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/force_data", 10, 
            std::bind(&ForceSubscriber::force_callback, this, std::placeholders::_1));
        
        RCLCPP_INFO(this->get_logger(), "Force subscriber started");
    }

private:
    void force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
        // 打印接收到的力数据
        // RCLCPP_INFO(this->get_logger(), "Received force data: X=%.2f, Y=%.2f, Z=%.2f",msg->twist.linear.x,msg->twist.linear.y,msg->twist.linear.z);

        // 检查Z方向的力是否大于等于5N，并更新全局变量
        if (std::abs(msg->twist.linear.z) >= 5.0) {  // 使用abs因为力可能是负值
            force_sign = false;
        } else {
            force_sign = true;
        }

        // RCLCPP_INFO(this->get_logger(), "force_sign: %s", force_sign ? "true" : "false");
    }

    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr subscription_;
};

bool executeCartesianPath1(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    if (!returnToMiddle(move_group, logger)) {
        RCLCPP_ERROR(logger, "Failed to return to middle position before executing Cartesian path.");
        return false;
    }
    RCLCPP_INFO(logger, "Starting Cartesian path execution");

/* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    // [保持原有waypoints定义不变]
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = 0.1038815445000545;
    pose1.position.y = 0.05599530875132761;
    pose1.position.z = 0.287593164792294;
    pose1.orientation.x = -7.443283672575919e-06;
    pose1.orientation.y = 7.57226530269504e-05;
    pose1.orientation.z = 0.12184966603307737;
    pose1.orientation.w = 0.9925485646044253;
    waypoints.push_back(pose1);

    geometry_msgs::msg::Pose pose2;
    pose2.position.x = 0.08365577107241515;
    pose2.position.y = 0.1086784487646846;
    pose2.position.z = 0.2876367813618629;
    pose2.orientation.x = 1.760144636797022e-06;
    pose2.orientation.y = 5.322223642645702e-07;
    pose2.orientation.z = 0.24192347154193786;
    pose2.orientation.w = 0.970295333346356;
    waypoints.push_back(pose2);

    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.024580082964917316;
    pose3.position.y = 0.05115885226571315;
    pose3.position.z = 0.14427007528930358;
    pose3.orientation.x = -0.09252984740619437;
    pose3.orientation.y = 0.3713226058418401;
    pose3.orientation.z = 0.22355615291928813;
    pose3.orientation.w = 0.8964264589032053;
    waypoints.push_back(pose3);

    geometry_msgs::msg::Pose pose4;
    pose4.position.x = 0.0059124806471319985;
    pose4.position.y = 0.06736960543726173;
    pose4.position.z = 0.0851790937420965;
    pose4.orientation.x = -0.0787478196797887;
    pose4.orientation.y = 0.3158430604116397;
    pose4.orientation.z = 0.22880413581912243;
    pose4.orientation.w = 0.9174369784990976;
    waypoints.push_back(pose4);

    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.014876747557903927;
    pose5.position.y = 0.07209511922660344;
    pose5.position.z = 0.069480923286174;
    pose5.orientation.x = -0.08464301863551622;
    pose5.orientation.y = 0.33968405068949703;
    pose5.orientation.z = 0.22656194017166972;
    pose5.orientation.w = 0.9089114326319655;
    waypoints.push_back(pose5);

    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.032490128976992244;
    pose6.position.y = 0.08148829798108001;
    pose6.position.z = 0.08403494444521113;
    pose6.orientation.x = -0.0686576561182485;
    pose6.orientation.y = 0.275514230486597;
    pose6.orientation.z = 0.2319904963985905;
    pose6.orientation.w = 0.9303539351432134;
    waypoints.push_back(pose6);

    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.044606693998343105;
    pose7.position.y = 0.08792626579373217;
    pose7.position.z = 0.08060679535604816;
    pose7.orientation.x = -0.06055302337925643;
    pose7.orientation.y = 0.24294582246096078;
    pose7.orientation.z = 0.23423143748854183;
    pose7.orientation.w = 0.9393861252969676;
    waypoints.push_back(pose7);

    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.08050738757562798;
    pose8.position.y = 0.10705084955998226;
    pose8.position.z = 0.10661040040187303;
    pose8.orientation.x = -0.03783271841333385;
    pose8.orientation.y = 0.1517922939389349;
    pose8.orientation.z = 0.23904254249342105;
    pose8.orientation.w = 0.9583248133052252;
    waypoints.push_back(pose8);

    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.09048433218830905;
    pose9.position.y = 0.11233859740087646;
    pose9.position.z = 0.15069511834155236;
    pose9.orientation.x = -0.006308112047193989;
    pose9.orientation.y = 0.025320410227902945;
    pose9.orientation.z = 0.24191090140946767;
    pose9.orientation.w = 0.9699475245226156;
    waypoints.push_back(pose9);

    //第10个目标位姿 28/-75/145/-67/0 guaban
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.07891347645340688;
    pose10.position.y = 0.10620928378452835;
    pose10.position.z = 0.15352159805648152;
    pose10.orientation.x = -0.006335726731139758;
    pose10.orientation.y = 0.02542771707055573;
    pose10.orientation.z = 0.24195449779523703;
    pose10.orientation.w = 0.9699336630759976;
    waypoints.push_back(pose10);

    //28/-90/140/-46/0 刮板出来
    //28/-87/140/-46/0 刮板出来
    geometry_msgs::msg::Pose pose11;
    pose11.position.x = 0.05341474564016524;
    pose11.position.y = 0.09252098507666111;
    pose11.position.z = 0.1455771372769992;
    pose11.orientation.x = -0.014774327731087568;
    pose11.orientation.y = 0.05923164232719785;
    pose11.orientation.z = 0.24125709998639794;
    pose11.orientation.w = 0.9685392834023154;
    waypoints.push_back(pose11);

    //28/-90/140/-46/0 刮板出来平移
    geometry_msgs::msg::Pose pose12;
    pose12.position.x = 0.06785862276609957;
    pose12.position.y = 0.04701154110675846;
    pose12.position.z = 0.15585413287456126;
    pose12.orientation.x = -0.004247732537266147;
    pose12.orientation.y = 0.03460675290044033;
    pose12.orientation.z = 0.12175632173110766;
    pose12.orientation.w = 0.9919474419244655;
    waypoints.push_back(pose12);

    geometry_msgs::msg::Pose pose13;
    pose13.position.x = 0.3035704582748756;
    pose13.position.y = -4.6019088696090045e-05;
    pose13.position.z = 0.30536264212765296;
    pose13.orientation.x = 1.8345570569793093e-06;
    pose13.orientation.y = -7.180863568924419e-05;
    pose13.orientation.z = -6.589662785214818e-05;
    pose13.orientation.w = 0.9999999952488945;
    waypoints.push_back(pose13);
/* #endregion */

    move_group.setStartStateToCurrentState();
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "Trajectory 1 %.2f%% achieved", fraction * 100.0);
    
    scale_trajectory_speed(cartesian_plan,1.4);
    move_group.asyncExecute(cartesian_plan);
    // 添加循环实时检查 force_sign
    bool is_stopped = false;  // 检查机械臂是否在运动标志位
    moveit::planning_interface::MoveGroupInterface::Plan remaining_plan;

    while (rclcpp::ok() && !trajectory_completed) {  // 主循环，只要节点运行就持续检查
        if (!force_sign) {
            if (!is_stopped) {  // 如果尚未停止，则停止
                RCLCPP_INFO(logger, "Force exceeded threshold, stopping execution");
                move_group.stop();
                is_stopped = true;
            }
        } else {  // force_sign 为 true
            if (is_stopped) {  // 如果当前是停止状态且力恢复正常，继续执行
                if (continueCartesianPath(move_group, waypoints, logger, remaining_plan)) {
                    RCLCPP_INFO(logger, "力恢复正常，继续执行");
                    move_group.asyncExecute(remaining_plan);
                    is_stopped = false;  // 更新状态为运行中
                } else {
                    RCLCPP_INFO(logger, "没有剩余路径可继续，执行完成");
                    trajectory_completed = true;  // 设置标志，表示轨迹完成
                    break;  // 没有剩余路径可执行，退出循环
                }
            }
        }
        rclcpp::sleep_for(std::chrono::milliseconds(100));  // 每100ms检查一次
    }
    if (trajectory_completed) {
        RCLCPP_INFO(logger, "Trajectory execution fully completed, preparing to exit");
    }
    return true;
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    
    // 创建节点
    auto moveit_node = rclcpp::Node::make_shared("zc4_moveit_control");
    
    // 创建MoveGroupInterface对象
    moveit::planning_interface::MoveGroupInterface move_group(moveit_node, "zc4");
    
    // 创建力数据订阅节点
    auto force_subscriber = std::make_shared<ForceSubscriber>();
    
    // 创建多线程执行器
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(moveit_node);
    executor.add_node(force_subscriber);
    
    // 在单独的线程中执行Cartesian路径
    std::thread moveit_thread([&move_group, &moveit_node]() {
        executeCartesianPath1(move_group, moveit_node->get_logger());
    });
    
    // 运行执行器，处理订阅回调
    executor.spin();
    
    // 等待moveit线程完成
    moveit_thread.join();
    
    rclcpp::shutdown();
    return 0;
}